#include "Collision_detector.h"
#include "Rrt_planner.h"
#include <iostream>
#include <fstream>
#include <string>

Polygon	read_polygon(int num_vertices, std::istream* in)
{
	Polygon result;
	for (int i = 0; i < num_vertices; i++)
	{
		double x,y;
		*in >> x >> y;
		Point p(x,y,0);
		result.push_back(p);
	}

	return result;
}

Polygon	load_robot(const char* pi_szFileName)
{
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 

	int num_robots;
	*in >> num_robots;
	
	// IMPORTANT:
	// ---------
	// only the first robot is read while the others are ignored

	int num_vertices;
	*in >> num_vertices;
	Polygon result =  read_polygon(num_vertices, in);
		
	ifile.close();
	return result;
}

vector<Polygon>	load_obstacles(const char* pi_szFileName)
{
	vector<Polygon> result;
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 

	int num_obstacles;
	*in >> num_obstacles;

	if (num_obstacles != 0)
	{
		for (int i = 0; i < num_obstacles; i++)
		{
			int num_vertices;
			*in >> num_vertices;

			Polygon current =  read_polygon(num_vertices, in);
			result.push_back(current);
		}
	}
		
	ifile.close();
	return result;
}

vector<Point>	load_query(const char* pi_szFileName)
{

	vector<Point> result;
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 
		
	int num_robots;
	*in >> num_robots;

	int num_confs;
	*in >> num_confs;

	for (int i = 0; i < num_confs; i++)
	{		
		double x,y;
		double theta;
		*in >> x >> y >> theta;
		Point point(x,y,theta / 360);
		result.push_back(point);
	}
		
	ifile.close();
	return result;
}

void	save_path(const char* pi_szFileName, list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
		
	int robot_num = 1;
	fprintf(fOutput, "\n%d\n", robot_num);
		
	int size = 2*path.size()-1;
	fprintf(fOutput, "\n%d\n", size);

	if (path.size() == 0) 
	{
		fclose(fOutput);
		return; 
	}

	fprintf(fOutput, "%.2f %.2f %.2f \n", vec[0][0],vec[0][1],vec[0][2] * 360);

	for (int i = 1; i < vec.size(); i++)
	{
		double x_diff = vec[i][0] - vec[i-1][0];
		double y_diff = vec[i][1] - vec[i-1][1];
		double angle_diff = smallest_angle(vec[i-1],vec[i]);

		fprintf(fOutput, "0.00 0.00 %.2f \n", angle_diff * 360);
		fprintf(fOutput, "%.2f %.2f 0.00 \n", x_diff, y_diff);
	}
	fclose(fOutput);
}

void	save_path_2(const char* pi_szFileName, list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
		
	int robot_num = 1;
	fprintf(fOutput, "\n%d\n", robot_num);
		
	int size = path.size();
	fprintf(fOutput, "\n%d\n", size);

	if (size == 0) 
	{
		fclose(fOutput);
		return; 
	}

	fprintf(fOutput, "%.2f %.2f %.2f \n", vec[0][0],vec[0][1],vec[0][2] * 360);

	for (int i = 1; i < vec.size(); i++)
	{
		double x = vec[i][0] ;
		double y = vec[i][1] ;
		double angle = vec[i][2];

		fprintf(fOutput, "%.2f %.2f %.2f \n", x, y, angle * 360);
	}

	fclose(fOutput);
}

void	save_path_distance_angle(const char* pi_szFileName, list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
		
	int robot_num = 1;
	fprintf(fOutput, "\n%d\n", robot_num);
		
	int size = path.size();
	fprintf(fOutput, "\n%d\n", size);

	if (size == 0) 
	{
		fclose(fOutput);
		return; 
	}

	fprintf(fOutput, "%.2f %.2f %.2f \n", vec[0][0],vec[0][1],vec[0][2] * 360);

	for (int i = 1; i < vec.size(); i++)
	{
		double l_diff = distance_points(vec[i],vec[i-1]);
		double angle_diff = smallest_angle(vec[i-1],vec[i]);

		fprintf(fOutput, "%.2f %.2f \n", l_diff, angle_diff * 360);
	}

	fclose(fOutput);
}

int main(int argc, char *argv[])
{

	// CONSTANTS
	double	step_size = 0.002; // length of each local_planner step
	int		iteration_num = 1000000;
	int		connect_target_rate = 10; // what is it 
	
	vector<Polygon>		obstacles = load_obstacles("workspace.txt");
	Polygon				robot = load_robot("robot.txt");
	vector<Point>		query = load_query("query.txt");

	Collision_detector	col(&obstacles, robot, step_size);

	Rrt_planner planner(&col, query[0], query[1],iteration_num, connect_target_rate);
	list<Point> result = planner.run();

	//save_path_distance_angle("pathC.txt", result);
	save_path_2("pathB.txt", result);
	save_path("path.txt", result);
}
